//
// Copyright [2020] Nobleo Technology"  [legal/copyright]
//
#include <algorithm>
#include <iostream>
#include <list>
#include <string>
#include <vector>

#include "full_coverage_path_planner/spiral_stc.h"
#include <pluginlib/class_list_macros.h>

// register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::SpiralSTC, nav_core::BaseGlobalPlanner)

namespace full_coverage_path_planner
{
void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
    if (!initialized_)
    {
        // Create a publisher to visualize the plan
        ros::NodeHandle private_nh("~/");
        ros::NodeHandle nh, private_named_nh("~/" + name);
        plan_pub_ = private_named_nh.advertise<nav_msgs::Path>("plan", 1);
        // Try to request the cpp-grid from the cpp_grid map_server
        cpp_grid_client_ = nh.serviceClient<nav_msgs::GetMap>("static_map");
        // Define  robot radius (radius) parameter 机器人半径(radius)参数
        float robot_radius_default = 0.5f;
        private_named_nh.param<float>("robot_radius", robot_radius_, robot_radius_default);
        // Define  tool radius (radius) parameter 工具半径(radius)参数
        float tool_radius_default = 0.5f;
        private_named_nh.param<float>("tool_radius", tool_radius_, tool_radius_default);
        initialized_ = true;
    }
}

bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                         std::vector<geometry_msgs::PoseStamped> &plan)
{
    if (!initialized_)
    {
        ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }
    else ROS_INFO("Initialized!");
    clock_t begin = clock();
    Point_t startPoint;
    /********************** Get grid from server **********************/
    std::vector<std::vector<bool> > grid;
    nav_msgs::GetMap grid_req_srv;
    ROS_INFO("Requesting grid!!");
    if (!cpp_grid_client_.call(grid_req_srv))
    {
        ROS_ERROR("Could not retrieve grid from map_server");
        return false;
    }
    // 将ROS占用网格转换为内部网格表示，给定单个瓷砖的大小
    if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint))
    {
        ROS_ERROR("Could not parse retrieved grid");
        return false;
    }
#ifdef DEBUG_PLOT
    ROS_INFO("Start grid is:");
    std::list<Point_t> printPath;
    printPath.push_back(startPoint);
    printGrid(grid, grid, printPath);
#endif
    // 进行螺旋- stc(生成树覆盖)覆盖路径规划。
    std::list<Point_t> goalPoints = spiral_stc(grid, startPoint,
            spiral_cpp_metrics_.multiple_pass_counter, spiral_cpp_metrics_.visited_counter);
    ROS_INFO("naive cpp completed!");
    ROS_INFO("Converting path to plan");
    // 将点列表转换成ROS路径
    parsePointlist2Plan(start, goalPoints, plan);
    // 打印一些指标:
    spiral_cpp_metrics_.accessible_counter = spiral_cpp_metrics_.visited_counter
                                             - spiral_cpp_metrics_.multiple_pass_counter;
    spiral_cpp_metrics_.total_area_covered =
            (4.0 * tool_radius_ * tool_radius_) * spiral_cpp_metrics_.accessible_counter;
    ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter);
    ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter);
    ROS_INFO("Total accessible cells: %d", spiral_cpp_metrics_.accessible_counter);
    ROS_INFO("Total accessible area: %f", spiral_cpp_metrics_.total_area_covered);
    // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept
    // (also controlled by planner_frequency parameter in move_base namespace)
    ROS_INFO("Publishing plan!");
    publishPlan(plan);
    ROS_INFO("Plan published!");
    ROS_DEBUG("Plan published");
    clock_t end = clock();
    double elapsed_secs = static_cast<double>(end - begin) / CLOCKS_PER_SEC;
    ROS_INFO_STREAM("elapsed time: " << elapsed_secs);
    return true;
}

std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const &grid, Point_t &init,
        int &multiple_pass_counter, int &visited_counter)
{
    int x, y, nRows = grid.size(), nCols = grid[0].size();
    // Initial node is initially set as visited so it does not count
    multiple_pass_counter = 0;
    visited_counter = 0;
    // Copy grid matrix
    std::vector<std::vector<bool> > visited = grid;
    x = init.x;
    y = init.y;
    // 起始点
    Point_t new_point = {x, y};
    // Point: x,y ; Cost ; Heuristic
    gridNode_t new_node = {new_point, 0, 0,};
    std::list<gridNode_t> pathNodes;
    std::list<Point_t> fullPath;
    pathNodes.push_back(new_node);
    visited[y][x] = eNodeVisited;
#ifdef DEBUG_PLOT
    ROS_INFO("Grid before walking is: ");
printGrid(grid, visited, fullPath);
#endif
    // 第一次螺旋填充
    pathNodes = SpiralSTC::spiral(grid, pathNodes, visited);
    // 获取剩余的目标点
    std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);
    // Add points to full path
    std::list<gridNode_t>::iterator it;
    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
    {
        Point_t newPoint = {it->pos.x, it->pos.y};
        visited_counter++;
        fullPath.push_back(newPoint);
    }
    // Remove all elements from pathNodes list except last element
    pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
#ifdef DEBUG_PLOT
    ROS_INFO("Current grid after first spiral is");
printGrid(grid, visited, fullPath);
ROS_INFO("There are %d goals remaining", goals.size());
#endif
    while (!goals.empty())
    {
        // Remove all elements from pathNodes list except last element.
        // The last point is the starting point for a new search and A* extends the path from there on
        // 删除pathNodes列表中除最后一个元素外的所有元素。 最后一个点是新搜索的起始点，a *从那里扩展路径
        pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
        visited_counter--;  // First point is already counted as visited
        // Plan to closest open Node using A* `goals` is essentially the map,
        // so we use `goals` to determine the distance from the end of a potential path to the nearest free space
        // 计划到最近的开放节点使用A* '目标'本质上是地图，
        // 因此我们使用' goals '来确定从潜在路径的末端到最近的空闲空间的距离
        bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes);
        if (resign)
        {
#ifdef DEBUG_PLOT
            ROS_INFO("A_star_to_open_space is resigning", goals.size());
#endif
            break;
        }
        // Update visited grid
        for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
        {
            if (visited[it->pos.y][it->pos.x]) multiple_pass_counter++;
            visited[it->pos.y][it->pos.x] = eNodeVisited;
        }
        // First point is already counted as visited 第一个点已被计算为访问过
        if (!pathNodes.empty()) multiple_pass_counter--;
#ifdef DEBUG_PLOT
        ROS_INFO("Grid with path marked as visited is:");
    gridNode_t SpiralStart = pathNodes.back();
    printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
#endif
        // Spiral fill from current position 从当前位置螺旋填充
        pathNodes = spiral(grid, pathNodes, visited);
#ifdef DEBUG_PLOT
        ROS_INFO("Visited grid updated after spiral:");
    printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
#endif
        // Retrieve remaining goalPoints 找回剩余的得分点
        goals = map_2_goals(visited, eNodeOpen);
        for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
        {
            Point_t newPoint = {it->pos.x, it->pos.y};
            visited_counter++;
            fullPath.push_back(newPoint);
        }
    }
    return fullPath;
}

std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const &grid, std::list<gridNode_t> &init,
                                        std::vector<std::vector<bool> > &visited)
{
    int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
    // Spiral filling of the open space Copy incoming list to 'end'
    std::list<gridNode_t> pathNodes(init);
    // Create iterator for gridNode_t list and let it point to the last element of end
    auto it = --(pathNodes.end());
    // if list is length 1, keep iterator at end; Let iterator point to second to last element
    if (pathNodes.size() > 1) it--;
    gridNode_t prev = *(it);
    bool done = false;
    while (!done)
    {
        if (it != pathNodes.begin())
        {
            // turn ccw
            dx = pathNodes.back().pos.x - prev.pos.x;
            dy = pathNodes.back().pos.y - prev.pos.y;
            dx_prev = dx;
            dx = -dy;
            dy = dx_prev;
        }
        else
        {
            // Initialize spiral direction towards y-axis
            dx = 0;
            dy = 1;
        }
        done = true; // 0 1; 1 0; 0 -1; -1 0;
        for (int i = 0; i < 4; ++i)
        {
            x2 = pathNodes.back().pos.x + dx;
            y2 = pathNodes.back().pos.y + dy;
            if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows)
            {
                if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen)
                {
                    Point_t new_point = {x2, y2};
                    // Point: x,y ; Cost ; Heuristic
                    gridNode_t new_node = {new_point, 0, 0,};
                    prev = pathNodes.back();
                    pathNodes.push_back(new_node);
                    it = --(pathNodes.end());
                    visited[y2][x2] = eNodeVisited;  // Close node
                    done = false;
                    break;
                }
            }
            // try next direction cw 试试下一个方向cw
            dx_prev = dx;
            dx = dy;
            dy = -dx_prev;
        }
    }
    return pathNodes;
}

}  // namespace launch
